# encoding: utf-8

import matplotlib.pyplot as plt
from matplotlib.ticker import MultipleLocator
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import math
import re
from io import StringIO

plt.rcParams['font.sans-serif']=['SimHei'] # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False # 用来正常显示负号


# 替换固定格式内容  xxx: 兼容数据采集的文本分析
def loadFile(input_file_path):
    # 读取文件内容
    with open(input_file_path, 'r', encoding='utf-8') as file:
        content = file.read()

    # 使用正则表达式替换掉目标模式 [0-9]+: 匹配一个或多个数字后面跟着一个冒号
    updated_content = re.sub(r'\b[0-9]+:\s*', '', content)  # 注意这里增加\s*来匹配可能跟随的空格

    # 使用 StringIO 创建一个内存中的文件对象
    cleaned_file_like_obj = StringIO(updated_content)

    # 使用 numpy.loadtxt 从 StringIO 对象加载数据
    data = np.loadtxt(cleaned_file_like_obj, delimiter=',')
    return data

#使用numpy导入数据

file_name = "E:/crp_linux_pc/build_windows/bin/bin/Debug/pos/RobotPosture34.txt"

# 假设你要读取 RobotPosture28.txt, RobotPosture29.txt, 和 RobotPosture30.txt
# file_num = 3  
file_num = 8

data_list  = []
data_list .append(loadFile(file_name))

# 提取文件路径中的数字部分
if file_num  > 1:
    
    pattern = re.compile(r'(\d+)\.txt$')
    match = pattern.search(file_name)

    if not match:
        raise ValueError("File name does not contain a valid number.")


    start_number = int(match.group(1)) + 1
    base_path = file_name[:match.start()]  # 获取文件名前缀部分
    file_extension = ".txt"

    # 逐个读取文件并将数据添加到列表中
    for i in range(start_number, start_number + file_num -1):
        current_file_name = f"{base_path}{i}{file_extension}"
        try:
            data_list .append(loadFile(current_file_name))
        except FileNotFoundError:
            print(f"File {current_file_name} not found and will be skipped.")



# 将所有数据合并到一个NumPy数组中
data = np.vstack(data_list)

x = data[:,0]
y = data[:,1]
z = data[:,2]

# 时间间隔 (秒)
dt = 0.002

# 计算速度
vx = np.gradient(x) / dt
vy = np.gradient(y) / dt
vz = np.gradient(z) / dt

# 计算末端速度
end_effector_velocity = np.sqrt(vx**2 + vy**2 + vz**2)

# 创建图形
fig = plt.figure(figsize=(14, 7))

# 三维轨迹图
ax1 = fig.add_subplot(121, projection='3d')
ax1.plot(x, y, z, c='b')
ax1.set_title('机器人轨迹（毫米）')
ax1.set_xlabel('X (毫米)')
ax1.set_ylabel('Y (毫米)')
ax1.set_zlabel('Z (毫米)')

# 设置显示范围
if x.max() - x.min() < 1:
    ax1.set_xlim(x.min()-1,x.max()+1)
if y.max() - y.min() < 1:
    ax1.set_ylim(y.min()-1,y.max()+1)
if z.max() - z.min() < 1:
    ax1.set_zlim(z.min()-1,z.max()+1)

# 速度曲线图
ax2 = fig.add_subplot(122)
time_points = np.arange(len(end_effector_velocity)) * dt
ax2.plot(time_points, end_effector_velocity, c='r')
ax2.set_title('末端速度曲线')
ax2.set_xlabel('时间 (秒)')
ax2.set_ylabel('末端速度 (毫米/秒)')

plt.tight_layout()
plt.show()